/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "Avant.h"
#include "Avant_settings.h"
#include "owndebug.h"
#include "World.h"

CAvant::CAvant(CWorld* World, float x, float y, float heading, std::string name,std::string aMaCIGroup)
:CObject(World,NULL),
name(name),
gimi(),
mcs(&gimi),
 SpeedCtrlServer(&mcs,0),
 jointServer(&mcs,0),
 iPositionServer(&mcs,0),
 iSpeed(0.0),
 iAngularSpeed(0.0),
 iPositionTimer(0)

/*  // For old speed control:
,
SpeedCtrlServer1(&mcs,1),
SpeedCtrlServer2(&mcs,2),
SpeedCtrlServer3(&mcs,3)
*/
{
  float z=1.1;		// starting height of Avant

  y=-y;
 
  mcs.SetGroupName(World->GetBaseMaCIGroup()+"."+name);
  
  //materials
  WheelMaterial.look.color[0]=0.2;WheelMaterial.look.color[1]=0.2;WheelMaterial.look.color[2]=0.2;
  WheelMaterial.density=WHEEL_MASS/(WHEEL_RADIUS*WHEEL_RADIUS*M_PI*WHEEL_WIDTH);

  WheelMaterial.surface.mu=100;

  BodyMaterial.look.color[0]=0.1;BodyMaterial.look.color[1]=0.7;BodyMaterial.look.color[2]=0.1;
  BodyMaterial.density=(CENTRAL_CHASSIS_MASS+UPPER_CHASSIS_MASS)/(CENTRAL_CHASSIS_VOLUME+UPPER_CHASSIS_VOLUME);

  RearBodyMaterial.look.color[0]=0.1;RearBodyMaterial.look.color[1]=0.7;RearBodyMaterial.look.color[2]=0.1;
  RearBodyMaterial.density=REAR_CHASSIS_MASS/REAR_CHASSIS_VOLUME;

  BoomMaterial.look.color[0]=0.1;BoomMaterial.look.color[1]=0.7;BoomMaterial.look.color[2]=0.1;
  BoomMaterial.density=TOTAL_BOOM_MASS/TOTAL_BOOM_VOLUME;

  ScoopMaterial.look.color[0]=0.2;ScoopMaterial.look.color[1]=0.2;ScoopMaterial.look.color[2]=0.2;
  ScoopMaterial.density=TOTAL_SCOOP_MASS/TOTAL_SCOOP_VOLUME;

  EdgeMaterial.look.color[0]=0.3;EdgeMaterial.look.color[1]=0.3;EdgeMaterial.look.color[2]=0.3;
  EdgeMaterial.density=TOTAL_SCOOP_MASS/TOTAL_SCOOP_VOLUME;

  //bodies
  Chassis = dBodyCreate(this->WorldID);
  dMass Central_Chassis_Mass, Rear_Chassis_Mass, Upper_Chassis_Mass;
  dMass Chassis_Mass;
  dMassSetZero(&Chassis_Mass);

  Upper_Boom = dBodyCreate(this->WorldID);
  dMass Bottom_Upper_Boom_Mass, Top_Upper_Boom_Mass;
  dMass Upper_Boom_Mass;
  dMassSetZero(&Upper_Boom_Mass);

  Scoop = dBodyCreate(this->WorldID);
  dMass Scoop_Back_Mass, Scoop_Bottom_Mass, Cutting_Edge_Mass;
  dMass Scoop_Mass;
  dMassSetZero(&Scoop_Mass);

  //parts
  Parts.push_back(Central_Chassis=new CBox(CENTRAL_CHASSIS_WIDTH,CENTRAL_CHASSIS_HEIGHT,CENTRAL_CHASSIS_LENGTH,this,SpaceID,Chassis,&BodyMaterial));
  dMassSetBox(&Central_Chassis_Mass,BodyMaterial.density,CENTRAL_CHASSIS_WIDTH,CENTRAL_CHASSIS_HEIGHT,CENTRAL_CHASSIS_LENGTH);

  Parts.push_back(Rear_Chassis=new CBox(REAR_CHASSIS_WIDTH,REAR_CHASSIS_HEIGHT,REAR_CHASSIS_LENGTH,this,SpaceID,Chassis,&RearBodyMaterial));
  dMassSetBox(&Rear_Chassis_Mass,RearBodyMaterial.density,REAR_CHASSIS_WIDTH,REAR_CHASSIS_HEIGHT,REAR_CHASSIS_LENGTH);

  Parts.push_back(Upper_Chassis=new CBox(UPPER_CHASSIS_WIDTH,UPPER_CHASSIS_HEIGHT,UPPER_CHASSIS_LENGTH,this,SpaceID,Chassis,&BodyMaterial));
  dMassSetBox(&Upper_Chassis_Mass,BodyMaterial.density,UPPER_CHASSIS_WIDTH,UPPER_CHASSIS_HEIGHT,UPPER_CHASSIS_LENGTH);

  Parts.push_back(Bottom_Upper_Boom=new CBox(BOTTOM_UPPER_BOOM_WIDTH,BOTTOM_UPPER_BOOM_HEIGHT,BOTTOM_UPPER_BOOM_LENGTH,this,SpaceID,Upper_Boom,&BoomMaterial));
  dMassSetBox(&Bottom_Upper_Boom_Mass,BoomMaterial.density,BOTTOM_UPPER_BOOM_WIDTH,BOTTOM_UPPER_BOOM_HEIGHT,BOTTOM_UPPER_BOOM_LENGTH);
  
  Parts.push_back(Top_Upper_Boom=new CBox(TOP_UPPER_BOOM_WIDTH,TOP_UPPER_BOOM_HEIGHT,TOP_UPPER_BOOM_LENGTH,this,SpaceID,Upper_Boom,&BoomMaterial));
  dMassSetBox(&Top_Upper_Boom_Mass,BoomMaterial.density,TOP_UPPER_BOOM_WIDTH,TOP_UPPER_BOOM_HEIGHT,TOP_UPPER_BOOM_LENGTH);

  Parts.push_back(Lower_Boom=new CBox(LOWER_BOOM_WIDTH,LOWER_BOOM_HEIGHT,LOWER_BOOM_LENGTH,this,SpaceID,0,&BoomMaterial));

  Parts.push_back(Scoop_Back=new CBox(SCOOP_BACK_WIDTH,SCOOP_BACK_HEIGHT,SCOOP_BACK_LENGTH,this,SpaceID,Scoop,&ScoopMaterial));
  dMassSetBox(&Scoop_Back_Mass,ScoopMaterial.density,SCOOP_BACK_WIDTH,SCOOP_BACK_HEIGHT,SCOOP_BACK_LENGTH);

  Parts.push_back(Scoop_Bottom=new CBox(SCOOP_BOTTOM_WIDTH,SCOOP_BOTTOM_HEIGHT,SCOOP_BOTTOM_LENGTH,this,SpaceID,Scoop,&ScoopMaterial));
  dMassSetBox(&Scoop_Bottom_Mass,ScoopMaterial.density,SCOOP_BOTTOM_WIDTH,SCOOP_BOTTOM_HEIGHT,SCOOP_BOTTOM_LENGTH);

  Parts.push_back(Cutting_Edge=new CBox(CUTTING_EDGE_WIDTH,CUTTING_EDGE_HEIGHT,CUTTING_EDGE_LENGTH,this,SpaceID,Scoop,&EdgeMaterial));
  dMassSetBox(&Cutting_Edge_Mass,EdgeMaterial.density,CUTTING_EDGE_WIDTH,CUTTING_EDGE_HEIGHT,CUTTING_EDGE_LENGTH);

  //Scoop_Bottom->NotifyCollisions=true; //so that we can implement digging in the onCollision callback
  Cutting_Edge->NotifyCollisions=true; //so that we can implement digging in the onCollision callback
  
  Parts.push_back(RightFrontWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,this,SpaceID,0,&WheelMaterial));
  Parts.push_back(LeftFrontWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,this,SpaceID,0,&WheelMaterial));
  Parts.push_back(RightRearWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,this,SpaceID,0,&WheelMaterial));
  Parts.push_back(LeftRearWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,this,SpaceID,0,&WheelMaterial));

  
  //set mass positions and orientations
  dMatrix3 R;
  const dReal *P;

  dMassTranslate(&Central_Chassis_Mass,0,CENTRAL_CHASSIS_Y,-CENTRAL_CHASSIS_Z);
  dMassTranslate(&Rear_Chassis_Mass,0,REAR_CHASSIS_Y,-REAR_CHASSIS_Z);
  dMassTranslate(&Upper_Chassis_Mass,0,UPPER_CHASSIS_Y,-UPPER_CHASSIS_Z);
  dRFromAxisAndAngle(R, 1, 0, 0, DegToRad(-45));
  dMassRotate(&Upper_Chassis_Mass, R);
  dMassTranslate(&Bottom_Upper_Boom_Mass,0,BOTTOM_UPPER_BOOM_Y,-BOTTOM_UPPER_BOOM_Z);
  dMassRotate(&Bottom_Upper_Boom_Mass, R);
  dMassTranslate(&Top_Upper_Boom_Mass,0,TOP_UPPER_BOOM_Y,-TOP_UPPER_BOOM_Z);
  dMassRotate(&Top_Upper_Boom_Mass, R);
  dMassTranslate(&Scoop_Back_Mass,0,SCOOP_BACK_Y,-SCOOP_BACK_Z);
  dMassTranslate(&Scoop_Bottom_Mass,0,SCOOP_BOTTOM_Y,-SCOOP_BOTTOM_Z);
  dMassTranslate(&Cutting_Edge_Mass,0,CUTTING_EDGE_Y,-CUTTING_EDGE_Z);

  //add component masses to composite objects
  dMassAdd(&Chassis_Mass,&Central_Chassis_Mass);
  dMassAdd(&Chassis_Mass,&Rear_Chassis_Mass);
  dMassAdd(&Chassis_Mass,&Upper_Chassis_Mass);
  dMassAdd(&Upper_Boom_Mass,&Bottom_Upper_Boom_Mass);
  dMassAdd(&Upper_Boom_Mass,&Top_Upper_Boom_Mass);
  dMassAdd(&Scoop_Mass,&Scoop_Back_Mass);
  dMassAdd(&Scoop_Mass,&Scoop_Bottom_Mass);
  dMassAdd(&Scoop_Mass,&Cutting_Edge_Mass);

  //set part positions and orientations
  dBodySetPosition(RightFrontWheel->BodyID,x+WHEEL_X,WHEEL_RADIUS+z,y-FRONT_WHEEL_Z);
  dRFromAxisAndAngle(R, 0, 1, 0, DegToRad(90));
  dBodySetRotation(RightFrontWheel->BodyID,R);

  dBodySetPosition(LeftFrontWheel->BodyID,x-WHEEL_X,WHEEL_RADIUS+z,y-FRONT_WHEEL_Z);
  dRFromAxisAndAngle(R, 0, 1, 0, DegToRad(-90));
  dBodySetRotation(LeftFrontWheel->BodyID,R);

  dBodySetPosition(RightRearWheel->BodyID,x+WHEEL_X,WHEEL_RADIUS+z,y-REAR_WHEEL_Z);
  dRFromAxisAndAngle(R, 0, 1, 0, DegToRad(90));
  dBodySetRotation(RightRearWheel->BodyID,R);

  dBodySetPosition(LeftRearWheel->BodyID,x-WHEEL_X,WHEEL_RADIUS+z,y-REAR_WHEEL_Z);
  dRFromAxisAndAngle(R, 0, 1, 0, DegToRad(-90));
  dBodySetRotation(LeftRearWheel->BodyID,R);

  dBodySetPosition(Chassis,x+Chassis_Mass.c[0],WHEEL_RADIUS+z+Chassis_Mass.c[1],y+Chassis_Mass.c[2]);

  dGeomSetOffsetPosition(Central_Chassis->GeomID,0-Chassis_Mass.c[0],CENTRAL_CHASSIS_Y-Chassis_Mass.c[1],-CENTRAL_CHASSIS_Z-Chassis_Mass.c[2]);
  dGeomSetOffsetPosition(Rear_Chassis->GeomID,0-Chassis_Mass.c[0],REAR_CHASSIS_Y-Chassis_Mass.c[1],-REAR_CHASSIS_Z-Chassis_Mass.c[2]);
  dGeomSetOffsetPosition(Upper_Chassis->GeomID,0-Chassis_Mass.c[0],UPPER_CHASSIS_Y-Chassis_Mass.c[1],-UPPER_CHASSIS_Z-Chassis_Mass.c[2]);
  dRFromAxisAndAngle(R, 1, 0, 0, DegToRad(-45));
  dGeomSetOffsetRotation(Upper_Chassis->GeomID,R);

  dBodySetPosition(Upper_Boom,x+Upper_Boom_Mass.c[0],WHEEL_RADIUS+z+Upper_Boom_Mass.c[1],y+Upper_Boom_Mass.c[2]);

  dGeomSetOffsetPosition(Bottom_Upper_Boom->GeomID,0-Upper_Boom_Mass.c[0],BOTTOM_UPPER_BOOM_Y-Upper_Boom_Mass.c[1],-BOTTOM_UPPER_BOOM_Z-Upper_Boom_Mass.c[2]);
  dRFromAxisAndAngle(R, 1, 0, 0, DegToRad(-45));
  dGeomSetOffsetRotation(Bottom_Upper_Boom->GeomID,R);
  dGeomSetOffsetPosition(Top_Upper_Boom->GeomID,0-Upper_Boom_Mass.c[0],TOP_UPPER_BOOM_Y-Upper_Boom_Mass.c[1],-TOP_UPPER_BOOM_Z-Upper_Boom_Mass.c[2]);
  dGeomSetOffsetRotation(Top_Upper_Boom->GeomID,R);

  dBodySetPosition(Lower_Boom->BodyID,x,WHEEL_RADIUS+z+LOWER_BOOM_Y,y-LOWER_BOOM_Z);
  dRFromAxisAndAngle(R, 1, 0, 0, DegToRad(-45));
  dBodySetRotation(Lower_Boom->BodyID,R);

  dBodySetPosition(Scoop,x+Scoop_Mass.c[0],WHEEL_RADIUS+z+Scoop_Mass.c[1],y+Scoop_Mass.c[2]);

  dGeomSetOffsetPosition(Scoop_Back->GeomID,0-Scoop_Mass.c[0],SCOOP_BACK_Y-Scoop_Mass.c[1],-SCOOP_BACK_Z-Scoop_Mass.c[2]);
  dGeomSetOffsetPosition(Scoop_Bottom->GeomID,0-Scoop_Mass.c[0],SCOOP_BOTTOM_Y-Scoop_Mass.c[1],-SCOOP_BOTTOM_Z-Scoop_Mass.c[2]);
  dGeomSetOffsetPosition(Cutting_Edge->GeomID,0-Scoop_Mass.c[0],CUTTING_EDGE_Y-Scoop_Mass.c[1],-CUTTING_EDGE_Z-Scoop_Mass.c[2]);

  //joints
  BoomRot=dJointCreateHinge(this->WorldID,0);
  BoomPrism=dJointCreateSlider(this->WorldID,0);
  ScoopRot=dJointCreateHinge(this->WorldID,0);

  LeftFrontMotor=dJointCreateHinge(this->WorldID,0);
  RightFrontMotor=dJointCreateHinge(this->WorldID,0);
  LeftRearMotor=dJointCreateHinge(this->WorldID,0);
  RightRearMotor=dJointCreateHinge(this->WorldID,0);

  //connect parts with joints
  //ATTACH JOINTS BEFORE SETTING PARAMS!!
  dJointAttach(BoomRot,Chassis,Upper_Boom);
  dJointAttach(BoomPrism,Upper_Boom,Lower_Boom->BodyID);
  dJointAttach(ScoopRot,Lower_Boom->BodyID,Scoop);
  dJointAttach(LeftFrontMotor,Chassis,LeftFrontWheel->BodyID);
  dJointAttach(RightFrontMotor,Chassis,RightFrontWheel->BodyID);
  dJointAttach(LeftRearMotor,Chassis,LeftRearWheel->BodyID);
  dJointAttach(RightRearMotor,Chassis,RightRearWheel->BodyID);

  P=dBodyGetPosition(Chassis);
  dJointSetHingeAxis(BoomRot,1,0,0);
  dJointSetHingeAnchor(BoomRot,P[0]-Chassis_Mass.c[0],P[1]+0.91-Chassis_Mass.c[1],P[2]+0.25-Chassis_Mass.c[2]);

  P=dBodyGetPosition(Lower_Boom->BodyID);
  dJointSetSliderAxis(BoomPrism,0,-1,-1);

  P=dBodyGetPosition(Chassis);
  dJointSetHingeAxis(ScoopRot,1,0,0);
  dJointSetHingeAnchor(ScoopRot,P[0]-Chassis_Mass.c[0],P[1]-Chassis_Mass.c[1],P[2]-0.78-Chassis_Mass.c[2]);

  P=dBodyGetPosition(LeftFrontWheel->BodyID);
  dJointSetHingeAxis(LeftFrontMotor,1,0,0);
  dJointSetHingeAnchor(LeftFrontMotor,P[0],P[1],P[2]);

  P=dBodyGetPosition(RightFrontWheel->BodyID);
  dJointSetHingeAxis(RightFrontMotor,1,0,0);
  dJointSetHingeAnchor(RightFrontMotor,P[0],P[1],P[2]);

  P=dBodyGetPosition(LeftRearWheel->BodyID);
  dJointSetHingeAxis(LeftRearMotor,1,0,0);
  dJointSetHingeAnchor(LeftRearMotor,P[0],P[1],P[2]);

  P=dBodyGetPosition(RightRearWheel->BodyID);
  dJointSetHingeAxis(RightRearMotor,1,0,0);
  dJointSetHingeAnchor(RightRearMotor,P[0],P[1],P[2]);

  dMassTranslate(&Chassis_Mass,-Chassis_Mass.c[0],-Chassis_Mass.c[1],-Chassis_Mass.c[2]);
  dMassTranslate(&Upper_Boom_Mass,-Upper_Boom_Mass.c[0],-Upper_Boom_Mass.c[1],-Upper_Boom_Mass.c[2]);
  dMassTranslate(&Scoop_Mass,-Scoop_Mass.c[0],-Scoop_Mass.c[1],-Scoop_Mass.c[2]);

  //set parent body mass
  dBodySetMass(Chassis,&Chassis_Mass);
  dBodySetMass(Upper_Boom,&Upper_Boom_Mass);
  dBodySetMass(Scoop,&Scoop_Mass);

  //set joint properties
  dJointSetHingeParam(LeftFrontMotor, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(LeftFrontMotor, dParamVel,0.0);

  dJointSetHingeParam(RightFrontMotor, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(RightFrontMotor, dParamVel,0.0);

  dJointSetHingeParam(LeftRearMotor, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(LeftRearMotor, dParamVel,0.0);

  dJointSetHingeParam(RightRearMotor, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(RightRearMotor, dParamVel,0.0);

  dJointSetHingeParam(BoomRot, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(BoomRot, dParamVel,0.0);
  //dJointSetHingeParam(BoomRot, dParamLoStop, -1.57);	//limit of boom rotation 
  //dJointSetHingeParam(BoomRot, dParamHiStop, 0.01);
  dJointSetHingeParam(BoomRot, dParamLoStop, -1.5);	//"0" for hi and lo freezes the joint
  dJointSetHingeParam(BoomRot, dParamHiStop, 0.01);

  dJointSetSliderParam(BoomPrism, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetSliderParam(BoomPrism, dParamVel,0.0);
  dJointSetSliderParam(BoomPrism, dParamLoStop, -0.7);	//limit - "0" for hi and lo freezes the joint
  dJointSetSliderParam(BoomPrism, dParamHiStop, 0.01);

  dJointSetHingeParam(ScoopRot, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(ScoopRot, dParamVel,0.0);
  dJointSetHingeParam(ScoopRot, dParamLoStop, -0.65);	//limit - "0" for hi and lo freezes the joint
  dJointSetHingeParam(ScoopRot, dParamHiStop, 3.0);

  dReal pos[3]={SCANNER_OFFSET_X,SCANNER_OFFSET_Z,0};
  dMatrix3 rot;
  dRFrom2Axes(rot, -1,0,0, 0,1,0);
  
  /*Children.push_back(laserScanner=new CSick(World,this,Chassis, pos, rot, 
  LASER_INTERVAL,&mcs,0,"Laser"));
  */
  Children.push_back(laserScanner=new CLaserScanner(World,this,Chassis, pos, rot, 
                     LASER_INTERVAL, 180, 80, -M_PI/2.0, M_PI/2.0, &mcs,0,"Laser"));
  

  //Setting macigroup...
  mcs.SetGroupName(World->GetBaseMaCIGroup()+"."+aMaCIGroup);
  connectGIMI();
  
}

CAvant::~CAvant()
{
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    delete (*i);
  }

  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    delete (*i);
  }
}

void CAvant::Activate()
{
  SpeedCtrlServer.SetInterfaceInstanceName("Motion");
  SpeedCtrlServer.SetSpeedLimits(3, 3, 1, -3, -3, 0.5);
  //SpeedCtrlServer.SetSpeedLimits(float aMaxSpeed, float aMaxAngularSpeed, float aMaxAcceleration,
    //                      float aMinSpeed, float aMinAngularSpeed, float aMinAcceleration);

  MaCI::EMaCIError e=SpeedCtrlServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open SpeedCtrlServer");
  }
/*
  // For old speed control:
  MaCI::EMaCIError e1=SpeedCtrlServer1.Open();
  if (e1!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open SpeedCtrlServer1");
  }
  MaCI::EMaCIError e2=SpeedCtrlServer2.Open();
  if (e2!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open SpeedCtrlServer2");
  }
  MaCI::EMaCIError e3=SpeedCtrlServer3.Open();
  if (e3!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open SpeedCtrlServer3");
  }
*/
  MaCI::JointGroupCtrl::TJointInformationArray array;
  #define MANIPULATOR_POSITION_CONTROL
  #ifdef MANIPULATOR_POSITION_CONTROL
  // JointGroupCtrl Position Control:
  MaCI::JointGroupCtrl::TJointInformation_Payload joint1(0,MaCI::JointGroupCtrl::KJointControlLinear,0,0,1,-1, 1);
  joint1.SetName(std::string("Telescope"));
  MaCI::JointGroupCtrl::TJointInformation_Payload joint2(1,MaCI::JointGroupCtrl::KJointControlAngular,0,0,1,-M_PI/2.0, M_PI/2.0);
  joint2.SetName(std::string("Boom (Puomi)"));
  MaCI::JointGroupCtrl::TJointInformation_Payload joint3(2,MaCI::JointGroupCtrl::KJointControlAngular,0,0,1,-M_PI/2.0, M_PI/2.0);
  joint3.SetName(std::string("Bucket (Kauha)"));
  #else
 // JointGroupCtrl Speed Control:
  /*  MaCI::JointGroupCtrl::TJointInformation_Payload joint1(0,MaCI::JointGroupCtrl::KJointControlLinear,1,1);
  joint1.SetName(std::string("Telescope"));
  MaCI::JointGroupCtrl::TJointInformation_Payload joint2(1,MaCI::JointGroupCtrl::KJointControlAngular,1,1);
  joint2.SetName(std::string("Boom (Puomi)"));
  MaCI::JointGroupCtrl::TJointInformation_Payload joint3(2,MaCI::JointGroupCtrl::KJointControlAngular,1,1);
  joint3.SetName(std::string("Bucket (Kauha)"));*/
  #endif

  array.push_back(joint1);
  array.push_back(joint2);
  array.push_back(joint3);
  jointServer.SetJointGroupInfo(array);
  jointServer.SetInterfaceInstanceName("NormalJoints");
  /// Create TEventTypes container
  MaCI::JointGroupCtrl::TEventTypes eventTypes;
  /// Add Position to be sent as an event
  eventTypes.SetEventType(MaCI::JointGroupCtrl::KTypeJointPosition);
  //Set it to the server
  jointServer.SetEventTypes(eventTypes);
  MaCI::EMaCIError e4=jointServer.Open();
  if ( e4 != MaCI::KMaCIOK) 
  {
    dPrint(1,"Failed to Open jointServer");
  }

  iPositionServer.SetInterfaceInstanceName("SimulatorPosition");
  e4 = iPositionServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open PositionServer");
  }

  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Activate();
  }
  
}

void CAvant::Update(float StepSize)
{
  //TODO: sivusuuntaisen kitkan muutos renkaisiin niiden pyoriessa
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Update(StepSize);
  }
  // Driving speed control
  if (SpeedCtrlServer.WaitCommand(SpeedCtrlData,0)==MaCI::KMaCIOK)
  {
    const MaCI::SpeedCtrl::TCommand *c=SpeedCtrlData.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c)
      switch (c->cmd)
        {
        case MaCI::SpeedCtrl::KCommandSetSpeed:
          s=SpeedCtrlData.GetSpeed();
          if (s)
            SetSpeed(s->speed,s->angularSpeed,s->acceleration);
          break;
        case MaCI::SpeedCtrl::KCommandSetStop:
          SetSpeed(0,0,10);
          break;
        case MaCI::SpeedCtrl::KCommandTimeout:
          SetSpeed(0,0,10);
          break;
      }
  }  
  MaCI::SpeedCtrl::CSpeedCtrlData speedEvent;
  gim::binbag::CBinBag bag;
  speedEvent.EncodeTo(&bag); 
  speedEvent.SetCommand(MaCI::SpeedCtrl::TCommand(MaCI::SpeedCtrl::KEventSpeed));
  speedEvent.SetSpeed(MaCI::SpeedCtrl::TSpeed(iSpeed,iAngularSpeed));
  //speedEvent.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
  speedEvent.SetTimestamp(MaCI::Common::TTimestamp());
  SpeedCtrlServer.SendSpeedEvent(speedEvent);

/*
// For old speed control of manipulator joints:
  if (SpeedCtrlServer1.WaitCommand(SpeedCtrlData,0)==MaCI::KMaCIOK)
  {
    dPrint(1,"Position 1");
    const MaCI::SpeedCtrl::TCommand *c=SpeedCtrlData.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c)
    {
    dPrint(1,"Position 2");
      switch (c->cmd)
      {
        case MaCI::SpeedCtrl::KCommandSetSpeed:
    	  dPrint(1,"Position 3");
          s=SpeedCtrlData.GetSpeed();
          if (s)
            {dPrint(1,"Getting boom commands: %f",s->speed);
            SetBoomRotSpeed(s->speed,s->angularSpeed,s->acceleration);}
          dPrint(1,"Position 4");
          break;
        case MaCI::SpeedCtrl::KCommandSetStop:
          SetBoomRotSpeed(0,0,10);
          dPrint(1,"At KCommandSetStop");
          break;
      }
    }
  }

  if (SpeedCtrlServer2.WaitCommand(SpeedCtrlData,0)==MaCI::KMaCIOK)
  {
    const MaCI::SpeedCtrl::TCommand *c=SpeedCtrlData.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c)
      switch (c->cmd)
      {
        case MaCI::SpeedCtrl::KCommandSetSpeed:
          s=SpeedCtrlData.GetSpeed();
          if (s)
            SetBoomPrismSpeed(s->speed,s->angularSpeed,s->acceleration);
          break;
        case MaCI::SpeedCtrl::KCommandSetStop:
          SetBoomPrismSpeed(0,0,10);
          break;
      }
  }

  if (SpeedCtrlServer3.WaitCommand(SpeedCtrlData,0)==MaCI::KMaCIOK)
  {
    const MaCI::SpeedCtrl::TCommand *c=SpeedCtrlData.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c)
      switch (c->cmd)
      {
        case MaCI::SpeedCtrl::KCommandSetSpeed:
          s=SpeedCtrlData.GetSpeed();
          if (s)
            SetScoopRotSpeed(s->speed,s->angularSpeed,s->acceleration);
          break;
        case MaCI::SpeedCtrl::KCommandSetStop:
          SetScoopRotSpeed(0,0,10);
          break;
      }
  }
*/
  MaCI::JointGroupCtrl::CJointGroupCtrlData jointData;
  MaCI::JointGroupCtrl::TReplyInfo replyInfo;


  while (jointServer.WaitCommand(jointData,replyInfo,0)==MaCI::KMaCIOK)
  {
    const MaCI::JointGroupCtrl::TCommand *command;
	//Received command, handle it!
	command = jointData.GetCommand();
	//dPrint(8,"Got MaCI message (%d)", command->cmd);
	// Switch based on command type
	switch(command->cmd) {
        #ifdef MANIPULATOR_POSITION_CONTROL
	// Position Control
	case MaCI::JointGroupCtrl::KCommandSetGroupPositions:{
          MaCI::JointGroupCtrl::TJointPositionPtrArray array = jointData.GetJointPositionPtrArray();
	  // Got SetPosition()
          for(EACH_IN_i(array)){
            dPrint(5,"COMMAND: SET_POSITION (%f, %d)",(*i)->position,(*i)->jointNumber);
            switch((*i)->jointNumber){
            case 0:
              
              //dPrint(5,"Setting telescope");
              desiredTelescopePosition = (*i)->position;
              break;
            case 1:
              
              //dPrint(5,"Setting boom");
              desiredBoomAngle = (*i)->position;
              break;
            case 2:
              //dPrint(5,"Setting bucket"); 
              //iAvant->SetBucketSpeed(speed->speed);
              //iAvant->Start();
              desiredScoopAngle = (*i)->position;
              break;
              
            default:
              dPrint(3,"Wrong joint number %d",(*i)->jointNumber);
              break;
            }
            break;
          }
          //Then send an event when joints are moved..
          gim::binbag::CBinBag bb2;
          MaCI::JointGroupCtrl::CJointGroupCtrlData eventData;
          eventData.EncodeTo(&bb2);
          eventData.SetCommand(MaCI::JointGroupCtrl::TCommand(MaCI::JointGroupCtrl::KCommandEvent));
         
          eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(desiredTelescopePosition,0));
          eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(desiredBoomAngle,1));
          eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(desiredScoopAngle,2));
          
          eventData.Print(10);
          jointServer.SendEvent(eventData);
        }

       
	case MaCI::JointGroupCtrl::KCommandUpdateState:{
          gim::binbag::CBinBag bb2;
          MaCI::JointGroupCtrl::CJointGroupCtrlData eventData;
          eventData.EncodeTo(&bb2);
          eventData.SetCommand(MaCI::JointGroupCtrl::TCommand(MaCI::JointGroupCtrl::KCommandEvent));
         
          eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(desiredTelescopePosition,0));
          eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(desiredBoomAngle,1));
          eventData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(desiredScoopAngle,2));
          
          eventData.Print(10);
          jointServer.SendEvent(eventData);
	}
        #else
          /*	// Speed Control
	case MaCI::JointGroupCtrl::KCommandSetGroupSpeed:{
          TJointSpeedPtrArray array = data.GetJointSpeedPtrArray();
          for(EACH_IN_i(array)){

            // Got SetSpeed()
            dPrint(5,"COMMAND: SET_SPEED (%f, %d)",
                   (*i)->speed,
                   (*i)->jointNumber);
            switch((*i)->jointNumber){
            case 0:
              
              dPrint(5,"Setting telescope");
              //iAvant->SetTelescope(speed->speed);
              //iAvant->Start();
              SetTelescopeJointSpeed((*i)->speed); 
              break;
            case 1:
              
              dPrint(5,"Setting boom");
              //iAvant->SetBoomSpeed(speed->speed);
              //iAvant->Start();
              SetBoomJointSpeed((*i)->speed); 
              break;
            case 2:
              dPrint(5,"Setting bucket"); 
              //iAvant->SetBucketSpeed(speed->speed);
              //iAvant->Start();
              SetBucketJointSpeed((*i)->speed); 
              break;
              
            default:
              dPrint(3,"Wrong joint number %d",speed->jointNumber);
              break;
            }
          }
	  break;
        }
	
	  // For speed control:
	case MaCI::JointGroupCtrl::KCommandGetSpeed:{
	  
	  const MaCI::JointGroupCtrl::TJointNumber *number = jointData.GetJointNumber();
	  dPrint(5,"COMMAND: GET_SPEED (%d)",
		 number->number);
	  
	  float speed = 0.0;
	  switch(number->number){
	  case 0:
	    //speed = iAvant->GetTelescopeSpeed();
	    break;
	  case 1:
	    //speed = iAvant->GetBoomSpeed();
	    break;
	  case 2:
	    //speed = iAvant->GetBucketSpeed();
	    break;
	  }
	  
	  dPrint(10,"Replying %f",speed);
	  
	  MaCI::JointGroupCtrl::CJointGroupCtrlData replyData;
	  replyData.EncodeTo(&binbag);
	  replyData.SetCommand(MaCI::JointGroupCtrl::TCommand(MaCI::JointGroupCtrl::KReplyGetSpeed));
	  replyData.SetSpeed(MaCI::JointGroupCtrl::TJointSpeed(speed,number->number));
	  jointServer.SendReply(replyData, replyInfo);
	  break;
	}
          */
        #endif
        case MaCI::JointGroupCtrl::KCommandGetJointGroupStatus:{
          dPrint(10,"COMMAND: GET_STATUS");
          //dPrint(10,"Replying %d(Open)",MaCI::JointGroupCtrl::KJointGroupStatusOpen);
          
          MaCI::JointGroupCtrl::CJointGroupCtrlData replyData;
          replyData.EncodeTo(&binbag);
          replyData.SetCommand(MaCI::JointGroupCtrl::TCommand(MaCI::JointGroupCtrl::KReplyGetJointGroupStatus));
          replyData.SetJointGroupStatus(MaCI::JointGroupCtrl::TJointGroupStatus(MaCI::JointGroupCtrl::KJointGroupStatusOpen));
          jointServer.SendReply(replyData, replyInfo);
          break;
        }
     }
  }

  /*MaCI::JointGroupCtrl::CJointGroupCtrlData replyData;
  replyData.EncodeTo(&binbag);
  replyData.SetCommand(MaCI::JointGroupCtrl::TCommand(MaCI::JointGroupCtrl::KReplyGetPosition));
  replyData.SetPosition(MaCI::JointGroupCtrl::TJointPosition(position,number->number));
  jointServer.SendReply(replyData, replyInfo);*/

  #ifdef MANIPULATOR_POSITION_CONTROL
  // Call control functions
  ControlBoomJoint();
  ControlTelescopeJoint();
  ControlScoopJoint();
  #endif


  //POSITION SERVER
  if (iPositionTimer>=POSITION_INTERVAL)
  {
    const dReal *P, *R;
    dReal v[3]={1,0,0};
    dReal r[3];
    float heading;
    P=dBodyGetPosition(Rear_Chassis->BodyID);
    R=dBodyGetRotation(Rear_Chassis->BodyID);
    RotateVector(R,v,r);
#warning "NOTE: Using inverted y-axis and heading"
    heading=atan2(-r[2],r[0])+M_PI/2.0;
    //TODO: fix this heading gludge stuff
    //float t=SimWorld->GetSimulationTime();
    MaCI::Position::CPositionData pd;
    pd.EncodeTo(&binbag);
    //    pd.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
    pd.SetTimestamp(MaCI::Common::TTimestamp());
    //dPrint(1,"odoposx=%f odoposy=%f odoheading=%f", odoPosX, odoPosY, odoHeading);
    pd.SetPose2D(MaCI::Position::TPose2D(P[0],-P[2],heading));
    pd.SetVariance2D(MaCI::Position::TVariance2D(0,0,0));
    pd.SetProbability(MaCI::Position::TProbability(1));

    iPositionServer.SendPositionEvent(pd);
    //pd.Print(1);
    iPositionTimer=0;
  }
  iPositionTimer+=StepSize;

}

void CAvant::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{
  //is scoop blade touching ground
  if (primitive==Cutting_Edge && numc && primitive2 && SimWorld->GetTerrain()->IsTerrain(primitive2))
  {
 
    //bounding box defining area that is touched
    float x_max=-1000000, x_min=1000000, y_max=-1000000, y_min=1000000;
    
    //find all contacts with ground and their bounding box for determining the area that is updated
    for (int i=0; i<numc; i++)
    {
      if (contacts[i].geom.pos[0] > x_max )
        x_max=contacts[i].geom.pos[0];
      
      if (contacts[i].geom.pos[2] > y_max )
        y_max=contacts[i].geom.pos[2];
      
      if (contacts[i].geom.pos[0] < x_min )
        x_min=contacts[i].geom.pos[0];
              
      if (contacts[i].geom.pos[2] < y_min )
        y_min=contacts[i].geom.pos[2];
        
    }
      //dPrint(1,"x_max before: %f", x_max);
    
      float block_x, block_y;
      float reso;
      int samples_x, samples_y;
    
      /*
      MaCI::Map::CMapData mapData;
      MaCI::Map::TMapHeader mapHeader;
      MaCI::Map::TMapUpdateHeader udHeader;
      MaCI::Map::TGridBlockHeader gridHeader;
      
      mapData.CreateInternalBinBag();
      */
      reso=SimWorld->GetTerrain()->GetHeightMapResolution();
      //dPrint(1,"reso: %f", reso);
     /*
      udHeader.clear=false;
      
      mapData.SetMapHeader(mapHeader);
      mapData.SetMapUpdateHeader(udHeader);
      */
      //threshold bounding box values to be multiples of height maps resolution (rounded down)
      x_max=x_max-fmod(x_max, reso);
      //dPrint(1,"x_max after: %f", x_max);
      y_max=y_max-fmod(y_max, reso);
      x_min=x_min-fmod(x_min, reso);
      y_min=y_min-fmod(y_min, reso);
      /*
      gridHeader.dataType=MaCI::Map::KGridTypeHeightMap;
      gridHeader.x=x_min-reso;
      gridHeader.y=y_min-reso;
      gridHeader.samples_x=lrint((x_max-x_min)/reso)+1;
      gridHeader.samples_y=lrint((y_max-y_min)/reso)+1;
      gridHeader.dataSize=gridHeader.samples_x*gridHeader.samples_y*sizeof(float);
      */
      //block_x=x_min-reso;
      //block_y=y_min-reso;
      block_x=x_min-reso;
      block_y=y_min-reso;
      samples_x=lrint((x_max-x_min)/reso)+1+2;
      samples_y=lrint((y_max-y_min)/reso)+1+2;
      dPrint(1,"samples_x %i", samples_x);
      dPrint(1,"samples_y %i", samples_y);
      
      float *data=new float[samples_x*samples_y];
      
      //get the ground model from affected area
      SimWorld->GetTerrain()->getGroundModel(block_x,-block_y, 
                                samples_x, samples_y, data);
      
      
      for (int i=0; i<numc; i++)
      {
      
        //threshold contact positions
        float cx=contacts[i].geom.pos[0]-fmod(contacts[i].geom.pos[0],reso);
        float cy=contacts[i].geom.pos[2]-fmod(contacts[i].geom.pos[2],reso);  
	float height=contacts[i].geom.pos[1];
  	//dPrint(1,"Height %f", height);
        
        int x=lrint( (cx-x_min)/reso ) +1;
        int y=lrint( (cy-y_min)/reso ) +1;
        
        //now finally, this is where the ground is modified!!
        //in this example, the ground is just lowered 5 cm in contact point:
        //data[y*samples_x+x]= data[y*samples_x+x]-0.05;
	//in this example, set ground to height of collision point:
        data[y*samples_x+x]= height-0.05;
	// try lowering the height of some neighbours too
	data[y*samples_x+x+1] = height-0.05;
	data[y*samples_x+x-1] = height-0.05;
	data[(y-1)*samples_x+x] = height-0.05;
	data[(y+1)*samples_x+x] = height-0.05;
      }
      
      
      //finally update terrain
      SimWorld->GetTerrain()->updateTerrain(block_x, -block_y, samples_x,
          samples_y,data);
      
      delete[] data;
    
    
  }
  
//   //is scoop blade touching ground
//   if (primitive==Scoop_Bottom && numc && primitive2 && SimWorld->GetTerrain()->IsTerrain(primitive2))
//   {
//  
//     //bounding box defining area that is touched
//     float x_max=-1000000, x_min=1000000, y_max=-1000000, y_min=1000000;
//     
//     //find all contacts with ground and their bounding box for determining the area that is updated
//     for (int i=0; i<numc; i++)
//     {
//       if (contacts[i].geom.pos[0] > x_max )
//         x_max=contacts[i].geom.pos[0];
//       
//       if (contacts[i].geom.pos[2] > y_max )
//         y_max=contacts[i].geom.pos[2];
//       
//       if (contacts[i].geom.pos[0] < x_min )
//         x_min=contacts[i].geom.pos[0];
//               
//       if (contacts[i].geom.pos[2] < y_min )
//         y_min=contacts[i].geom.pos[2];
//         
//     }
//     
//       float block_x, block_y;
//       float reso;
//       int samples_x, samples_y;
//     
//       /*
//       MaCI::Map::CMapData mapData;
//       MaCI::Map::TMapHeader mapHeader;
//       MaCI::Map::TMapUpdateHeader udHeader;
//       MaCI::Map::TGridBlockHeader gridHeader;
//       
//       mapData.CreateInternalBinBag();
//       */
//       reso=SimWorld->GetTerrain()->GetHeightMapResolution();
//       /*
//       udHeader.clear=false;
//       
//       mapData.SetMapHeader(mapHeader);
//       mapData.SetMapUpdateHeader(udHeader);
//       */
//       //treshold bounding box values to be multiples of maps resolution (rounded down)
//       x_max=x_max-fmod(x_max, reso);
//       y_max=y_max-fmod(y_max, reso);
//       x_min=x_min-fmod(x_min, reso);
//       y_min=y_min-fmod(y_min, reso);
//       /*
//       gridHeader.dataType=MaCI::Map::KGridTypeHeightMap;
//       gridHeader.x=x_min-reso;
//       gridHeader.y=y_min-reso;
//       gridHeader.samples_x=lrint((x_max-x_min)/reso)+1;
//       gridHeader.samples_y=lrint((y_max-y_min)/reso)+1;
//       gridHeader.dataSize=gridHeader.samples_x*gridHeader.samples_y*sizeof(float);
//       */
//       block_x=x_min-reso;
//       block_y=y_min-reso;
//       samples_x=lrint((x_max-x_min)/reso)+1;
//       samples_y=lrint((y_max-y_min)/reso)+1;
//       
//       float *data=new float[samples_x*samples_y];
//       
//       //get the ground model from affected area
//       SimWorld->GetTerrain()->getGroundModel(block_x,-block_y, 
//                                 samples_x, samples_y, data);
//       
//       
//       for (int i=0; i<numc; i++)
//       {
//       
//         //threshold contact positions
//         float cx=contacts[i].geom.pos[0]-fmod(contacts[i].geom.pos[0],reso);
//         float cy=contacts[i].geom.pos[2]-fmod(contacts[i].geom.pos[2],reso);  
//         
//         int x=lrint( (cx-x_min)/reso );
//         int y=lrint( (cy-y_min)/reso );
//         
//         //now finally, this is where the ground is modified!!
//         //in this example, the ground is just lowered 10 cm in contact point
//         data[y*samples_x+x]= data[y*samples_x+x]-0.1;
//         
//       }
//       
//       
//       //finally update terrain
//       SimWorld->GetTerrain()->updateTerrain(block_x, -block_y, samples_x,
//           samples_y,data);
//       
//       delete[] data;
//     
//     
//   }
}

void CAvant::Draw()
{
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Draw();
  }

  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    (*i)->Draw();
  }
}

void CAvant::SetSpeed(float speed, float angularSpeed, float acceleration)
{
  float leftVelocity = 0.0;
  float rightVelocity = 0.0;
  float ar, al;
  float r = WHEELBASE/2.0;

  

  rightVelocity = angularSpeed*r+speed;
  leftVelocity = 2.0*speed-rightVelocity;
  dPrint(10,"Set speed start, speed=%f a.speed=%f acc=%f , leftSpeed %f, rightspeed %f",speed,angularSpeed,acceleration,leftVelocity,rightVelocity);
  //dPrint(1,"1");
  if (leftVelocity >= -MAX_MOTOR_SPEED && leftVelocity <= MAX_MOTOR_SPEED &&
      rightVelocity >= -MAX_MOTOR_SPEED && rightVelocity <= MAX_MOTOR_SPEED)
  {
    ar=rightVelocity/(WHEEL_RADIUS);
    al=leftVelocity/(WHEEL_RADIUS);

    dJointSetHingeParam (RightFrontMotor, dParamVel, ar);
    dJointSetHingeParam (RightRearMotor, dParamVel, ar);
    dJointSetHingeParam (LeftFrontMotor, dParamVel, al);
    dJointSetHingeParam (LeftRearMotor, dParamVel, al);

    dJointSetHingeParam (RightFrontMotor, dParamFMax, MAX_MOTOR_TORQUE);
    dJointSetHingeParam (RightRearMotor, dParamFMax, MAX_MOTOR_TORQUE);
    dJointSetHingeParam (LeftFrontMotor, dParamFMax, MAX_MOTOR_TORQUE);
    dJointSetHingeParam (LeftRearMotor, dParamFMax, MAX_MOTOR_TORQUE);
    iSpeed = speed;
    iAngularSpeed = angularSpeed; 
  } else {
    dPrint(1,"Speeds out of range: %f or %f",
	  leftVelocity, rightVelocity);
  }
}
/*
// For old speed control of manipulator joints
void CAvant::SetBoomRotSpeed(float speed, float angularSpeed, float acceleration)
{
  //dPrint(1,"At SetBoomRotSpeed %f", speed);
  if (speed >= -MAX_MOTOR_SPEED && speed <= MAX_MOTOR_SPEED)
  {
    dJointSetHingeParam (BoomRot, dParamVel, speed);
    dJointSetHingeParam (BoomRot, dParamFMax, MAX_MOTOR_TORQUE);
    //dPrint(1,"Setting the Boom speed %f", speed);
  } else {
    dPrint(1,"Speed out of range: %f", speed);
  }
}
// For old speed control
void CAvant::SetBoomPrismSpeed(float speed, float angularSpeed, float acceleration)
{
  if (speed >= -MAX_MOTOR_SPEED && speed <= MAX_MOTOR_SPEED)
  {
    dJointSetSliderParam (BoomPrism, dParamVel, speed);
    dJointSetSliderParam (BoomPrism, dParamFMax, MAX_MOTOR_TORQUE);
  } else {
    dPrint(1,"Speed out of range: %f", speed);
  }
}
// For old speed control
void CAvant::SetScoopRotSpeed(float speed, float angularSpeed, float acceleration)
{
  //dPrint(1,"At SetScoopRotSpeed %f", speed);
  if (speed >= -MAX_MOTOR_SPEED && speed <= MAX_MOTOR_SPEED)
  {
    dJointSetHingeParam (ScoopRot, dParamVel, speed);
    dJointSetHingeParam (ScoopRot, dParamFMax, MAX_MOTOR_TORQUE/4);
    //dPrint(1,"Setting the Scoop speed %f", speed);
  } else {
    dPrint(1,"Speed out of range: %f", speed);
  }
}
*/
void CAvant::connectGIMI()
{
  /*if (gimi.readSettings("gimi_settings.xml"))
  {
    int r;
    r=gimi.connectToHubEx("",-1,name);
    if (r==GIMI_OK)
    {
      dPrint(1,"Connected using name %s",name.c_str());
    } else if (r==GIMI_INVALIDNAME){
      //wxString s=_T("Couldn't connect GIMI: Name ");
      //s << name << "already in use.";
      //wxMessageBox(s);
      dPrint(1,"Couldn't connect GIMI: Name '%s' already in use.", name.c_str());
    } else {
      //wxMessageBox("Couldn't connect GIMI: GIMI_ERROR.");
      dPrint(1,"Couldn't connect GIMI: GIMI_ERROR.");
    }

  } else {
    //wxMessageBox(_T("Couldn't read connection settings file (gimi_settings.xml)."));
    dPrint(1,"Couldn't read connection settings file (gimi_settings.xml).");
  }*/
  SimWorld->ConnectGimi(&gimi,name);
}

// For JointGroupControl Speed Control
void CAvant::SetTelescopeJointSpeed(float speed)
{
  if (speed >= -MAX_MOTOR_SPEED && speed <= MAX_MOTOR_SPEED)
  {
    dJointSetSliderParam (BoomPrism, dParamVel, speed);
    dJointSetSliderParam (BoomPrism, dParamFMax, MAX_TELESCOPE_FORCE);
  } else {
    dPrint(1,"Speed out of range: %f", speed);
  }
}
// For JointGroupControl Speed Control
void CAvant::SetBoomJointSpeed(float speed)
{
  if (speed >= -MAX_MOTOR_SPEED && speed <= MAX_MOTOR_SPEED)
  {
    dJointSetHingeParam (BoomRot, dParamVel, speed);
    dJointSetHingeParam (BoomRot, dParamFMax, MAX_BOOM_TORQUE);
    //dPrint(1,"Setting the Boom speed %f", speed);
  } else {
    dPrint(1,"Speed out of range: %f", speed);
  }
}
// For JointGroupControl Speed Control
void CAvant::SetBucketJointSpeed(float speed)
{
  if (speed >= -MAX_MOTOR_SPEED && speed <= MAX_MOTOR_SPEED)
  {
    dJointSetHingeParam (ScoopRot, dParamVel, speed);
    dJointSetHingeParam (ScoopRot, dParamFMax, MAX_SCOOP_TORQUE);
    //dPrint(1,"Setting the Scoop speed %f", speed);
  } else {
    dPrint(1,"Speed out of range: %f", speed);
  }
} 

// GetPosition functions for position control:
float CAvant::GetTelescopeJointPosition()
{
    return dJointGetSliderPosition(BoomPrism);
}

float CAvant::GetBoomJointPosition()
{
    return dJointGetHingeAngle(BoomRot);
}

float CAvant::GetScoopJointPosition()
{
    return dJointGetHingeAngle(ScoopRot);
}
// Control functions for position control:
void CAvant::ControlBoomJoint()		// proportional control
{
   currentBoomAngle = GetBoomJointPosition();
   boomJointError = desiredBoomAngle - currentBoomAngle;
   //dPrint(1,"Joint error: %f", boomJointError);
   dJointSetHingeParam (BoomRot, dParamVel, 5*boomJointError);
   dJointSetHingeParam (BoomRot, dParamFMax, MAX_BOOM_TORQUE);
}

void CAvant::ControlTelescopeJoint()	// proportional control
{
   currentTelescopePosition = GetTelescopeJointPosition();
   telescopeJointError = desiredTelescopePosition - currentTelescopePosition;
   //dPrint(1,"Joint error: %f", telescopeJointError);
   dJointSetSliderParam (BoomPrism, dParamVel, 5*telescopeJointError);
   dJointSetSliderParam (BoomPrism, dParamFMax, MAX_TELESCOPE_FORCE);
}

void CAvant::ControlScoopJoint()	// proportional control
{
   currentScoopAngle = GetScoopJointPosition();
   scoopJointError = desiredScoopAngle - currentScoopAngle;
   //dPrint(1,"Joint error: %f", scoopJointError);
   dJointSetHingeParam (ScoopRot, dParamVel, 5*scoopJointError);
   dJointSetHingeParam (ScoopRot, dParamFMax, MAX_SCOOP_TORQUE);
}

/*//TODO: doesnt work, rotations done wrong
void CAvant::setPanAndTilt()//float pan, float tilt)
{
  dMatrix3 R;
  
  dRFromEulerAngles(R,DegToRad(90),pan,tilt);
  
  dGeomSetOffsetRotation(box->GeomID,R);
}*/

